#include "FEM2DVector.h"
#include <time.h>
#include <fstream>
#include <Eigen/SVD>
#include <numeric>
using namespace std;

CFEM2DVector::CFEM2DVector(int n):
	m_nSizeVec(n)
{
	cout << "=============================================================" << endl;
	cout << "****                                                     ****" << endl;
	cout << "****                   GenFEM Version 0.1                ****" << endl;
	cout << "****                                                     ****" << endl;
	cout << "=============================================================" << endl;
	cout << "> About: \tGenFEM is a general package for finite element\n"
		<< "         \twritten in C++" << endl;
	cout << "> Author: \tYizhao Wan, Qingdao Institute of Marine Geology\n"
		<< "           \tChina Geological Survey, Shandong, China " << endl;
	cout << "> Release: \ton Jan 11, 2021" << endl;
	cout << "> Website: " << endl;
	cout << "> Email: \twanyizhao@mail.cgs.gov.cn" << endl;
	cout << "=============================================================" << endl;
	
}
CFEM2DVector::~CFEM2DVector()
{

}
void CFEM2DVector::Init()
{
	
	cout << "GenFEM Initializing..." << endl;
	for (unsigned int i = 0; i < m_Mesh.getAllBoundEdges().size(); ++i)
	{
		for (unsigned int j = 0; j < m_Mesh.getAllBoundEdges()[i].size(); ++j)
		{
			m_Mesh.getAllBoundEdges()[i][j].value.resize(m_nSizeVec);
		}
	}
	for (unsigned int i = 0; i < m_Mesh.getAllBoundNodes().size(); ++i)
	{
		for (unsigned int j = 0; j < m_Mesh.getAllBoundNodes()[i].size(); ++j)
		{
			m_Mesh.getAllBoundNodes()[i][j].value.resize(m_nSizeVec);
		}
	}

}

Rsv CFEM2DVector::generate_Initial(double(*fun)(CPoint), vector<CPoint> Pb)
{
	Rsv ini_value(Pb.size());
	for (unsigned int i = 0; i < Pb.size(); ++i)
	{
		ini_value(i) = (fun(Pb[i]));
	}
	return ini_value;
}

vector<CPoint> &CFEM2DVector::getPbTest()
{
	return getMesh().getPbTest();
}
vector<CPoint> &CFEM2DVector::getPbTrial()
{
	return getMesh().getPbTrial();
}
vector<CElement> &CFEM2DVector::getTbTest()
{
	return getMesh().getTbTest();
}
vector<CElement> &CFEM2DVector::getTbTrial()
{
	return getMesh().getTbTrial();
}
vector<CElement> &CFEM2DVector::getT()
{
	return getMesh().getElements();
}
vector<CPoint> &CFEM2DVector::getP()
{
	return getMesh().getPoints();
}

double CFEM2DVector::getMatCondNum(SpMat A)
{
	return 0;
}
///@details This function has to set the value of the matrix one by one, but the access to the sparse matrix with coeReff(i,j) is very slow.
///So we use the iterator to access the element in the matrix.
///Loop over the cols of the matrix, and get the inneriterator of that col
///Loop over the col (with iterator++ operator)
///If the iterator's row is equal to the inputed index, the index value is the value we want to set.
void CFEM2DVector::imposeFixValue(const int &index, const double &value,  SpMat &A,  Rsv &b)
{
	//clock_t start, end;
	cout << "Impose fix value in the point " << index +1 << endl;
	for (int k = 0; k < A.outerSize(); ++k)
	{
		//inner iterator of a column
		//it is more efficient to use iterator
		for (SparseMatrix<double>::InnerIterator it(A, k); it; ++it)
		{			
			//if the iterator's row is equal to the index then the iterator point to the row which we want to manipulate
			if (it.row() == index)
			{
				it.valueRef() = 0;	
				break;
			}			
		}		
	}
	A.coeffRef(index, index) = 1;
	b[index] = value;
	cout << endl;

}

Rsv CFEM2DVector::solveLinearEquaParallel(const SpMat  &A, const Rsv   &b, const int &num_of_pro)
{
	//Solve the linear equations with Eigen
	clock_t start, end;
	cout << "Solve the linear equations with MKL parallel speed up with " << num_of_pro<<" processors" << endl;
	start = clock();

	Eigen::initParallel();
	Eigen::PardisoLU<SpMat> solver;
	mkl_set_dynamic(num_of_pro);
	solver.compute(A);
	if (solver.info() != Eigen::Success)
	{
		cout << "\nerror: Decomposition failed when using Eigen3 to compute matrix A\n";
	}
	Rsv x = solver.solve(b);
	if (solver.info() != Eigen::Success)
	{
		cout << "\nerror: Solving failed when using Eigen3 to solve Ax=b\n";
	}
	end = clock();
	cout << "Time consumption of linear equations solving:\t" << (double)(end - start) / CLOCKS_PER_SEC << " sec" << endl << endl;
	return x;
	
}

void CFEM2DVector::assignBoundEdgesValue(unsigned int bound_index, int vec_index, BOUNDARY_TYPE type, double(*value)(CPoint, double t), double(*r)(CPoint, double t))
{
	cout << "Assign boundary edges value success for boundary\t" << bound_index + 1 << endl;
	if (vec_index >= m_nSizeVec)
	{
		cout<<"Error: boundary's dimension is greater than the maximum dimension of the unknowns"<<endl;
		exit(100);
		
	}
	if (bound_index>=m_Mesh.getAllBoundEdges().size())
	{
		cout << "Error: boundary's index is greater than the maximum number of boundary" << endl;
		exit(100);
	}
	m_Mesh.assignBoundEdgesValue(m_Mesh.getAllBoundEdges()[bound_index], vec_index, type, value, r);
	
}

void CFEM2DVector::assignBoundNodesValue(unsigned int bound_index, int vec_index, BOUNDARY_TYPE type, double(*value)(CPoint, double t))
{
	cout << "Assign boundary nodes value success for " << vec_index + 1 << " value of boundary " << bound_index + 1 << endl;
	if (vec_index >= m_nSizeVec)
	{
		cout << "Error: boundary's dimension is greater than the maximum dimension of the unknowns" << endl;
		exit(100);
		
	}
	if (bound_index >= m_Mesh.getAllBoundNodes().size())
	{
		cout << "Error: boundary's index is greater than the maximum number of boundary" << endl;
		exit(100);
	}
	m_Mesh.assignBoundNodesValue(m_Mesh.getAllBoundNodes()[bound_index], vec_index, type, value);
	

}

void CFEM2DVector::setNumVec(int n)
{
	m_nSizeVec = n;
}

int CFEM2DVector::getNumVec()
{
	return m_nSizeVec;
}
CMesh & CFEM2DVector::getMesh()
{
	return m_Mesh;
}

///@details The basis functions are inputed by the caller, and it can be any kind of basis function defined in CBasisFunction.
///The Tb_trial and Tb_test are also inputed by the caller, it should be noted that the Tb must be consist with the basis function.
SpMat CFEM2DVector::assembleMatrix2D(double(*coe)(CPoint, double t), double t,
	CBasisFunction &basis_trial, const int &basis_trial_der_x, const int &basis_trial_der_y, vector<CElement>&Tb_trial,
	CBasisFunction &basis_test, const int &basis_test_der_x, const int &basis_test_der_y, vector<CElement>&Tb_test, const pair<int, int> &matrix_size)
{
	clock_t start, end;
	cout << "\nAssemble matrix" << endl;
	start = clock();

	int estimation_of_entries = m_Mesh.getTbTrial()[0].get_ele_node_num()*m_Mesh.getTbTrial().size(); //estimation of the non-zero values
	vector<Tri> tripletList;//a triple list for the matrix to improve the efficiency of sparse matrix manipulation
	tripletList.reserve(estimation_of_entries);

	for (unsigned int n = 0; n < m_Mesh.getTbTrial().size(); ++n)
	{
		vector<CPoint> vertices;//vertices (coordinates) of nth element(mesh nodes not finite element nodes)		
		for (int i = 0; i < m_Mesh.getElements()[n].get_ele_node_num(); ++i)
		{
			vertices.emplace_back(m_Mesh.getPoints()[m_Mesh.getElements()[n].getIds()[i]]);
		}

		vector<double> Gauss_weights;
		vector<CPoint> Gauss_nodes;
		//generate the Gauss integration points	

		generateGaussLocal2D(vertices, m_Mesh.getElements()[n].get_ele_type(), m_GaussType, Gauss_nodes, Gauss_weights);


		for ( int alpha = 0; alpha < Tb_trial[n].getIds().size(); ++alpha)
			for ( int beta = 0; beta < Tb_test[n].getIds().size(); ++beta)
			{
				double int_value = GaussQuad2DCoeTrialTest(coe, t, Gauss_weights, Gauss_nodes, vertices,
					basis_trial, alpha, basis_trial_der_x, basis_trial_der_y,
					basis_test, beta, basis_test_der_x, basis_test_der_y);

				int i = Tb_test[n].getIds()[beta];
				int j = Tb_trial[n].getIds()[alpha];
				tripletList.emplace_back(Tri(i, j, int_value));
			}
	}
	SpMat A(matrix_size.first, matrix_size.second);
	A.setFromTriplets(tripletList.begin(), tripletList.end());
	end = clock();
	cout << "Time consumption of matrix assemble:\t" << (double)(end - start) / CLOCKS_PER_SEC << " sec" << endl;
	return A;

}

///@details The The basis functions are inputed by the caller, and it can be any kind of basis function defined in CBasisFunction.
///The input Tb_test should be consist with the basis function.
Rsv CFEM2DVector::assembleVector2D(double(*load)(CPoint, double t), double t,
	CBasisFunction & basis_test, const int &basis_der_x_test, const int &basis_der_y_test, vector<CElement> &Tb_test)
{
	clock_t start, end;
	cout << "\nAssemble right side vector" << endl;
	start = clock();
	Rsv b = Rsv(m_Mesh.getPbTest().size());

	b.setZero();

	for (unsigned int n = 0; n < m_Mesh.getTbTest().size(); ++n)
	{
		vector<CPoint> vertices;//vertices of nth element
		vector<double> Gauss_weights;
		vector<CPoint> Gauss_nodes;

		for (int i = 0; i < m_Mesh.getElements()[n].get_ele_node_num(); ++i)
		{
			vertices.emplace_back(m_Mesh.getPoints()[m_Mesh.getElements()[n].getIds()[i]]);
		}
		//generate the Gauss integration points
		generateGaussLocal2D(vertices, m_Mesh.getElements()[n].get_ele_type(), m_GaussType, Gauss_nodes, Gauss_weights);

		for ( int beta = 0; beta <basis_test.getNumberBasis(); ++beta)
		{
			double int_value = GaussQuad2DLoadTrial(load, t, Gauss_weights, Gauss_nodes, vertices, basis_test, beta, basis_der_x_test, basis_der_y_test);

			b[Tb_test[n].getIds()[beta]] += int_value;
		}
	}
	end = clock();

	cout << "Time consumption of right side vector assemble:\t" << (double)(end - start) / CLOCKS_PER_SEC << " sec" << endl << endl;

	return b;

}

///@details The basis test and trial function is defined by the caller, and we can input different kinds of basis function.
double CFEM2DVector::GaussQuad2DCoeTrialTest(double(*coe)(CPoint, double t), double t, const vector<double> &Gauss_weights, const vector<CPoint> &Gauss_nodes, const vector<CPoint> &vertices,
	 CBasisFunction &basis_trial, const int &basis_index_trial, const int &basis_der_x_trial, const int &basis_der_y_trial,
	 CBasisFunction &basis_test, const int &basis_index_test, const int &basis_der_x_test, const int &basis_der_y_test)
{
	int Gpn = Gauss_weights.size();
	double int_value = 0;

	for (int k = 0; k < Gpn; ++k)
	{
		int_value += Gauss_weights[k] * coe(Gauss_nodes[k], t)
			*basis_trial.FE_basis_local_func_2D(Gauss_nodes[k], vertices, basis_trial.getType(), basis_index_trial, basis_der_x_trial, basis_der_y_trial)
			*basis_test.FE_basis_local_func_2D(Gauss_nodes[k], vertices, basis_test.getType(), basis_index_test, basis_der_x_test, basis_der_y_test);
	}
	return int_value;
}
double CFEM2DVector::GaussQuad2DLoadTrial(double(*load)(CPoint, double t), double t, const vector<double> &Gauss_weights, const vector<CPoint> &Gauss_nodes, const vector<CPoint> &vertices,
	CBasisFunction &basis_test, const int &basis_index_test, const int &basis_der_x_test, const int &basis_der_y_test)
{
	int Gpn = Gauss_weights.size();
	double int_value = 0;
	for (int k = 0; k < Gpn; ++k)
	{
		int_value += Gauss_weights[k] * load(Gauss_nodes[k], t)*basis_test.FE_basis_local_func_2D(Gauss_nodes[k], vertices, basis_test.getType(), basis_index_test, basis_der_x_test, basis_der_y_test);
	}
	return int_value;
}

///@details There are three ways to treat the Dirichlet boundary conditions.
///First one: set the A(i,i)=1 and A(i,j)=0, b=g(Pb(i)) to handle Dirichlet boundary.
///It is needed a loop over the column, and the manipulation function coeffRef() of matrix is severely time consuming.
///This method is very slow.\n
///Second one: Access the element of the matrix by interator like what we do in CFEM2DVector::imposeFixValue. \n
///This method is faster than the first one.
///Third one: b(i)=b=g(Pb(i))*A(i,i)*large_number,A(i,i)=A(i,i)*large_number
///It is only needed to manipulate the diagonal element of the matrix and it is very fast.
void CFEM2DVector::treatBoundaryConditions(double(*coe)(CPoint, double t), double t, SpMat & A, Rsv & b,vector<unsigned int> num_values)
{
	clock_t start, end;
	cout << "Treat boundary conditions" << endl;
	start = clock();
	for (unsigned int k = 0; k < m_Mesh.getAllBoundNodes().size(); ++k)
	{
		for (unsigned int l = 0; l < m_Mesh.getAllBoundNodes()[k].size(); ++l)
		{
			if (Dirichlet == m_Mesh.getAllBoundNodes()[k][l].type)
			{
				/***************************************************************************/
				//method 1:
				//set the A(i,i)=1 and A(i,j)=0, b=g(Pb(i)) to handle Dirichlet boundary
				//It is needed a loop over the column, and the manipulation function coeffRef() of matrix is severely time consuming 		

				/*int i = m_Mesh.getAllBoundNodes()[k][l].glo_node_index;
				for (unsigned int num = 0; num < m_Mesh.getAllBoundNodes()[k][l].mark.size(); ++num)
				{
					int index = m_Mesh.getAllBoundNodes()[k][l].mark[num];
					for (int j = 0; j < A.cols(); ++j)
					{
						A.coeffRef(i + accumulate(num_values.begin(), num_values.begin() + index, 0), j) = 0;
					}
					A.coeffRef(i + accumulate(num_values.begin(), num_values.begin() + index, 0), i + accumulate(num_values.begin(), num_values.begin() + index, 0)) = 1;
					b[i + accumulate(num_values.begin(), num_values.begin() + index, 0)] = m_Mesh.getAllBoundNodes()[k][l].value[index](m_Mesh.getPbTrial()[i], t);
				}*/

				/************************************************************************/
				//method 2
				//another faster method to access the the elements by iterator
				/*int i = m_Mesh.getAllBoundNodes()[k][l].glo_node_index;
				for (unsigned int num = 0; num < m_Mesh.getAllBoundNodes()[k][l].mark.size(); ++num)
				{
					int index = m_Mesh.getAllBoundNodes()[k][l].mark[num];

					for (int k = 0; k < A.outerSize(); ++k)
					{
						//inner iterator of a column
						//it is more efficient to use iterator
						for (SparseMatrix<double>::InnerIterator it(A, k); it; ++it)
						{
							//if the iterator's row is equal to the index then the iterator point to the row which we want to manipulate
							if (it.row() == i + accumulate(num_values.begin(), num_values.begin() + index, 0))
							{
								it.valueRef() = 0;
								break;
							}
						}
					}		
					
					b(i + accumulate(num_values.begin(), num_values.begin() + index, 0)) =m_Mesh.getAllBoundNodes()[k][l].value[index](m_Mesh.getPbTrial()[i], t);
					A.coeffRef(i + accumulate(num_values.begin(), num_values.begin() + index, 0), i + accumulate(num_values.begin(), num_values.begin() + index, 0)) = 1;
				}*/


				/*********************************************************************/
				//method 3
				//So we use the the second way to handle Dirichlet boundary
				//another way to handle the Dirichlet boundary
				//b(i)=b=g(Pb(i))*A(i,i)*large_number
				//A(i,i)=A(i,i)*large_number
				//It is only needed to manipulate the diagonal element of the matrix

				int i = m_Mesh.getAllBoundNodes()[k][l].glo_node_index;
				double large_number = 1e50;
				for (unsigned int num=0;num< m_Mesh.getAllBoundNodes()[k][l].mark.size();++num)
				{
					int index = m_Mesh.getAllBoundNodes()[k][l].mark[num];
					//Eigen::SparseMatrix<double>::InnerIterator it(A, i+num* m_Mesh.getPbTrial().size());
					double A_ii = A.coeffRef(
						i + accumulate(num_values.begin(), num_values.begin()+index,0), 
						i + accumulate(num_values.begin(), num_values.begin() + index, 0));
					b(i+ accumulate(num_values.begin(), num_values.begin() + index, 0)) 
						= A_ii *large_number*m_Mesh.getAllBoundNodes()[k][l].value[index](m_Mesh.getPbTrial()[i], t);
					A.coeffRef(
						i+ accumulate(num_values.begin(), num_values.begin() + index, 0), 
						i+ accumulate(num_values.begin(), num_values.begin() + index, 0)) = A_ii * large_number;
				}
				
			}
		}

	}

	for (unsigned int k = 0; k < m_Mesh.getAllBoundEdges().size(); ++k)
	{
		for (unsigned int l = 0; l < m_Mesh.getAllBoundEdges()[k].size(); ++l)
		{
			if (Neumann == m_Mesh.getAllBoundEdges()[k][l].type)
			{
				int nk = m_Mesh.getAllBoundEdges()[k][l].ele_index;
				int p1 = m_Mesh.getAllBoundEdges()[k][l].p.first;
				int p2 = m_Mesh.getAllBoundEdges()[k][l].p.second;

				//This vertices is the boundary edge's two nodes
				//It is for the generation of Gauss points on the edges
				vector<CPoint> vertices_ede{
					m_Mesh.getPbTest()[p1],
					m_Mesh.getPbTest()[p2]
				};
				vector<CPoint>Gauss_nodes;
				vector<double>Gauss_wights;
				generateGaussLocal1D(vertices_ede, m_GaussType, Gauss_nodes, Gauss_wights);

				//The basis function needs the vertices of the element not the edge
				//So it is needed to get the nk's nodes
				vector<CPoint> vertices;//vertices (coordinates) of nth element(mesh nodes not finite element nodes)		
				for (int i = 0; i < m_Mesh.getElements()[nk].get_ele_node_num(); ++i)
				{
					vertices.emplace_back(m_Mesh.getPoints()[m_Mesh.getElements()[nk].getIds()[i]]);
				}
				for (unsigned int num = 0; num < m_Mesh.getAllBoundEdges()[k][l].mark.size(); ++num)
				{
					int index = m_Mesh.getAllBoundEdges()[k][l].mark[num];
					for (unsigned int beta = 0; beta < m_Mesh.getTbTest()[nk].getIds().size(); ++beta)
					{
						double ini_value = GaussQuad1DBoundTrial(
							coe, t, m_Mesh.getAllBoundEdges()[k][l].value[index], Gauss_wights, Gauss_nodes, vertices, 
							m_Mesh.getTbTrial()[nk].getBasisType(ELE_BASIS), beta, 0, 0);
						b(m_Mesh.getTbTest()[nk].getIds()[beta]+ accumulate(num_values.begin(), num_values.begin() + index, 0)) += ini_value;
					}
				}
			}
			else if (Robin == m_Mesh.getAllBoundEdges()[k][l].type)
			{
				int nk = m_Mesh.getAllBoundEdges()[k][l].ele_index;
				int p1 = m_Mesh.getAllBoundEdges()[k][l].p.first;
				int p2 = m_Mesh.getAllBoundEdges()[k][l].p.second;

				//This vertices is the boundary edge's two nodes
				//It is for the generation of Gauss points on the edges
				vector<CPoint> vertices_ede{
					m_Mesh.getPbTest()[p1],
					m_Mesh.getPbTest()[p2]
				};
				vector<CPoint>Gauss_nodes;
				vector<double>Gauss_wights;
				generateGaussLocal1D(vertices_ede, m_GaussType, Gauss_nodes, Gauss_wights);

				//The basis function needs the vertices of the element not the edge
				//So it is needed to get the nk's nodes
				vector<CPoint> vertices;//vertices (coordinates) of nth element(mesh nodes not finite element nodes)		
				for (int i = 0; i < m_Mesh.getElements()[nk].get_ele_node_num(); ++i)
				{
					vertices.emplace_back(m_Mesh.getPoints()[m_Mesh.getElements()[nk].getIds()[i]]);
				}
				for (unsigned int num = 0; num < m_Mesh.getAllBoundEdges()[k][l].mark.size(); ++num)
				{
					int index = m_Mesh.getAllBoundEdges()[k][l].mark[num];
					for (unsigned int beta = 0; beta < m_Mesh.getTbTest()[nk].getIds().size(); ++beta)
					{
						double ini_value = GaussQuad1DBoundTrial(coe, t, 
							m_Mesh.getAllBoundEdges()[k][l].value[index], Gauss_wights, Gauss_nodes, vertices, 
							m_Mesh.getTbTrial()[nk].getBasisType(ELE_BASIS), beta, 0, 0);
						b(m_Mesh.getTbTest()[nk].getIds()[beta]+ accumulate(num_values.begin(), num_values.begin() + index, 0)) += ini_value;

						for (unsigned int alpha = 0; alpha < m_Mesh.getTbTrial()[nk].getIds().size(); ++alpha)
						{
							double ini_value = GaussQuad1DBoundTrialTest(coe, t, m_Mesh.getAllBoundEdges()[k][l].r[index], Gauss_wights, Gauss_nodes, vertices,
								m_Mesh.getTbTrial()[nk].getBasisType(ELE_BASIS), alpha, 0, 0, m_Mesh.getTbTest()[nk].getBasisType(ELE_BASIS), beta, 0, 0);
							int i = m_Mesh.getTbTest()[nk].getIds()[beta];
							int j = m_Mesh.getTbTrial()[nk].getIds()[alpha];
							A.coeffRef(
								i+ accumulate(num_values.begin(), num_values.begin() + index, 0), 
								j+ accumulate(num_values.begin(), num_values.begin() + index, 0)) += ini_value;

						}
					}
				}
			}
		}

	}
	end = clock();
	cout << "Time consumption of boundary conditions treatment :\t" << (double)(end - start) / CLOCKS_PER_SEC << " sec" << endl << endl;

}
SpMat CFEM2DVector::matrix_hstack(SpMat const &m1, SpMat const &m2)
{
	/*SpMat m1_hstack_m2(m1.rows(), m1.cols() + m2.cols());
	m1_hstack_m2.leftCols(m1.cols()) = m1;
	m1_hstack_m2.rightCols(m2.cols()) = m2;
	return m1_hstack_m2;*/

	SparseMatrix<double> M(m1.rows(), m1.cols()+m2.cols());
	M.reserve(m1.nonZeros() + m2.nonZeros());
	for (Index c = 0; c < m1.cols(); ++c)
	{
		M.startVec(c); // Important: Must be called once for each rows before inserting!
		for (SparseMatrix<double>::InnerIterator itL(m1, c); itL; ++itL)
			M.insertBack(itL.row(), c) = itL.value();		
	}
	for (Index c = 0; c <m2.cols(); ++c)
	{
		M.startVec(c+m1.cols()); // Important: Must be called once for each rows before inserting!		
		for (SparseMatrix<double>::InnerIterator itC(m2, c); itC; ++itC)
			M.insertBack(itC.row(), c + m1.cols()) = itC.value();
	}
	M.finalize();
	return M;


}
SpMat CFEM2DVector::matrix_hstack(SpMat const & m1, SpMat const & m2, SpMat const & m3)
{
	SparseMatrix<double> M(m1.rows(), m1.cols() + m2.cols() + m3.cols());
	M.reserve(m1.nonZeros() + m2.nonZeros() + m3.nonZeros());
	for (Index c = 0; c < m1.cols(); ++c)
	{
		M.startVec(c); // Important: Must be called once for each rows before inserting!
		for (SparseMatrix<double>::InnerIterator itL(m1, c); itL; ++itL)
			M.insertBack(itL.row(), c) = itL.value();
	}
	for (Index c = 0; c < m2.cols(); ++c)
	{
		M.startVec(c + m1.cols()); // Important: Must be called once for each rows before inserting!		
		for (SparseMatrix<double>::InnerIterator itC(m2, c); itC; ++itC)
			M.insertBack(itC.row(), c + m1.cols()) = itC.value();
	}
	for (Index c = 0; c < m3.cols(); ++c)
	{
		M.startVec(c + m1.cols()+m2.cols()); // Important: Must be called once for each rows before inserting!		
		for (SparseMatrix<double>::InnerIterator itP(m3, c); itP; ++itP)
			M.insertBack(itP.row(), m1.cols() + m2.cols()+c) = itP.value();
	}
	M.finalize();
	return M;
}
Eigen::SparseMatrix<double> CFEM2DVector::matrix_vstack(SpMat const &m1, SpMat const &m2)
{
	/*Eigen::SparseMatrix<double, Eigen::RowMajor> m1_vstack_m2(m1.rows() + m2.rows(), m1.cols());
	m1_vstack_m2.topRows(m1.rows()) = m1;
	m1_vstack_m2.bottomRows(m2.rows()) = m2;
	cout << m1_vstack_m2.IsRowMajor << endl;
	return m1_vstack_m2;*/

	SparseMatrix<double> M(m1.rows() + m2.rows(), m1.cols());
	M.reserve(m1.nonZeros() + m2.nonZeros());
	for (Index c = 0; c < m1.cols(); ++c)
	{
		M.startVec(c); // Important: Must be called once for each column before inserting!
		for (SparseMatrix<double>::InnerIterator itL(m1, c); itL; ++itL)
			M.insertBack(itL.row(), c) = itL.value();
		for (SparseMatrix<double>::InnerIterator itC(m2, c); itC; ++itC)
			M.insertBack(itC.row() + m1.rows(), c) = itC.value();
	}
	M.finalize();
	return M;
}

SpMat CFEM2DVector::matrix_vstack(SpMat const & m1, SpMat const & m2, SpMat const & m3)
{
	SparseMatrix<double> M(m1.rows() + m2.rows()+m3.rows(), m1.cols());
	M.reserve(m1.nonZeros() + m2.nonZeros()+m3.nonZeros());
	for (Index c = 0; c < m1.cols(); ++c)
	{
		M.startVec(c); // Important: Must be called once for each column before inserting!
		for (SparseMatrix<double>::InnerIterator itL(m1, c); itL; ++itL)
			M.insertBack(itL.row(), c) = itL.value();
		for (SparseMatrix<double>::InnerIterator itC(m2, c); itC; ++itC)
			M.insertBack(itC.row() + m1.rows(), c) = itC.value();
		for (SparseMatrix<double>::InnerIterator itP(m3, c); itP; ++itP)
			M.insertBack(itP.row() + m1.rows()+ m2.rows(), c) = itP.value();
	}
	M.finalize();
	return M;
}

Rsv CFEM2DVector::vector_stack(const Rsv &b1, const Rsv &b2)
{
	Rsv b12(b1.rows() + b2.rows());
	b12 << b1, b2;
	return b12;
}
Rsv CFEM2DVector::vector_stack( const Rsv&b1, const Rsv  &b2,const  Rsv &b3)
{
	Rsv b(b1.rows() + b2.rows()+b3.rows());
	b << b1, b2, b3;
	return b;
}



void CFEM2DVector::export2VTK(string filename,const Rsv &x)
{
	cout << "\nExport results to vtk visualized with Paraview" << endl;
	map<ELE_TYPE, int> type_ele_vtk = {
		{ELE_3,5},
		{ELE_4,9},
		{ELE_6,22},
		{ELE_9,28}
	};
	ofstream f(filename);
	f << "# vtk DataFile Version 3.0" << endl;
	f << "Result" << endl;
	f << "ASCII" << endl;
	f << "DATASET UNSTRUCTURED_GRID" << endl;

	f << "POINTS \t" << m_Mesh.getPbTest().size() << "\tdouble" << endl;
	for (unsigned int i = 0; i < m_Mesh.getPbTest().size(); ++i)
	{
		f << m_Mesh.getPbTest()[i].getX() << "\t" << m_Mesh.getPbTest()[i].getY() << "\t" << m_Mesh.getPbTest()[i].getZ() << endl;
	}
	f << "CELLS \t " << m_Mesh.getTbTest().size() << "\t" << (m_Mesh.getTbTest()[0].getIds().size() + 1)* m_Mesh.getTbTest().size() << endl;
	for (unsigned int i = 0; i < m_Mesh.getTbTest().size(); ++i)
	{

		f << m_Mesh.getTbTest()[i].getIds().size() << "\t";

		if (m_Mesh.getTbTest()[i].get_ele_type() == ELE_3 || m_Mesh.getTbTest()[i].get_ele_type() == ELE_4)
		{
			for (unsigned int j = 0; j < m_Mesh.getTbTest()[i].getIds().size(); ++j)
			{
				f << m_Mesh.getTbTest()[i].getIds()[j] << "\t";
			}
		}
		else if (m_Mesh.getTbTest()[i].get_ele_type() == ELE_6)
		{
			for (unsigned int j = 0; j < m_Mesh.getTbTest()[i].getIds().size(); j += 2)
			{
				f << m_Mesh.getTbTest()[i].getIds()[j] << "\t";
			}
			for (unsigned int j = 1; j < m_Mesh.getTbTest()[i].getIds().size(); j += 2)
			{
				f << m_Mesh.getTbTest()[i].getIds()[j] << "\t";
			}
		}
		else if (m_Mesh.getTbTest()[i].get_ele_type() == ELE_9)
		{
			for (unsigned int j = 0; j < m_Mesh.getTbTest()[i].getIds().size() - 1; j += 2)
			{
				f << m_Mesh.getTbTest()[i].getIds()[j] << "\t";
			}
			for (unsigned int j = 1; j < m_Mesh.getTbTest()[i].getIds().size() - 1; j += 2)
			{
				f << m_Mesh.getTbTest()[i].getIds()[j] << "\t";
			}
			f << m_Mesh.getTbTest()[i].getIds()[8] << "\t";
		}
		f << endl;

	}
	f << "CELL_TYPES\t" << m_Mesh.getTbTest().size() << endl;

	for (unsigned int i = 0; i < m_Mesh.getTbTest().size(); ++i)
	{
		f << type_ele_vtk[m_Mesh.getTbTest()[i].get_ele_type()] << endl;
	}

	f << "POINT_DATA\t" << m_Mesh.getPbTest().size() << endl;

	f << "VECTORS s_fem double" << endl;	
	for (unsigned int i = 0; i < m_Mesh.getPbTest().size(); ++i)
	{

		f << x(i) << "\t";
		f << x(i + m_Mesh.getPbTest().size()) << "\t";
		f << 0 << endl;

	}
	f.close();

}

void CFEM2DVector::export2VTK(string filename,const Rsv &x, vector<CPoint> &P, vector<CElement> &T)
{
	cout << "\nExport results to vtk visualized with Paraview" << endl;
	map<ELE_TYPE, int> type_ele_vtk = {
		{ELE_3,5},
		{ELE_4,9},
		{ELE_6,22},
		{ELE_9,28}
	};
	ofstream f(filename);
	f << "# vtk DataFile Version 3.0" << endl;
	f << "Result" << endl;
	f << "ASCII" << endl;
	f << "DATASET UNSTRUCTURED_GRID" << endl;

	f << "POINTS \t" <<P.size() << "\tdouble" << endl;
	for (unsigned int i = 0; i < P.size(); ++i)
	{
		f << P[i].getX() << "\t" << P[i].getY() << "\t" << P[i].getZ() << endl;
	}
	f << "CELLS \t " << T.size() << "\t" << (T[0].getIds().size() + 1)* T.size() << endl;
	for (unsigned int i = 0; i < T.size(); ++i)
	{

		f << T[i].getIds().size() << "\t";

		if (T[i].get_ele_type() == ELE_3 || T[i].get_ele_type() == ELE_4)
		{
			for (unsigned int j = 0; j < T[i].getIds().size(); ++j)
			{
				f << T[i].getIds()[j] << "\t";
			}
		}
		else if (T[i].get_ele_type() == ELE_6)
		{
			for (unsigned int j = 0; j < T[i].getIds().size(); j += 2)
			{
				f << T[i].getIds()[j] << "\t";
			}
			for (unsigned int j = 1; j < T[i].getIds().size(); j += 2)
			{
				f << T[i].getIds()[j] << "\t";
			}
		}
		else if (T[i].get_ele_type() == ELE_9)
		{
			for (unsigned int j = 0; j < T[i].getIds().size() - 1; j += 2)
			{
				f << T[i].getIds()[j] << "\t";
			}
			for (unsigned int j = 1; j < T[i].getIds().size() - 1; j += 2)
			{
				f << T[i].getIds()[j] << "\t";
			}
			f << T[i].getIds()[8] << "\t";
		}
		f << endl;

	}
	f << "CELL_TYPES\t" << T.size() << endl;

	for (unsigned int i = 0; i < T.size(); ++i)
	{
		f << type_ele_vtk[T[i].get_ele_type()] << endl;
	}

	f << "POINT_DATA\t" << P.size() << endl;
	f << "SCALARS s_fem double 1" << endl;
	f << "LOOKUP_TABLE default" << endl;
	for (unsigned int i = 0; i < P.size(); ++i)
	{
		f << x(i) << endl;	
	}
	f.close();
}
